%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Yifan Zhang
% Bejing University of Technology
% Copy Right 2023
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Use Hough Spectrum to rotate the map
% 
% 使用霍夫频谱和之前得到的旋转角度得到最终的旋转角度，并
% 输出旋转后的地图
%
% INPUT: 
%   H1, H2：霍夫参数空间矩阵，其行和列分别对应于 rho 和 theta 值。
%   im_occ2：需要旋转的地图的占据栅格地图
%   theta_cells：360
%   delta_ang：之前求出的估计角度值
% OUTPUT
%   ang2_cc_rot：应该是最小的角度值在矩阵中的索引，但是我觉得就是最小角度值
%   R2_cc_rot：-H，它是一个参数空间矩阵，其行和列分别对应于 rho 和 theta 值。 
%   T2_cc_rot：theta，x 轴和 rho 向量之间的角度，以度为单位，以数值矩阵形式返回
%   D2_cc_rot：rho，沿垂直于线条的向量从原点到线条的距离，以数值数组形式返回。
%   rot_hat：旋转角度 
%   im_occ2_cc_rot：旋转后的占据栅格地图
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [ang2_cc_rot, R2_cc_rot, T2_cc_rot, D2_cc_rot, rot_hat, im_occ2_cc_rot] = hs_compute_rot(H1, H2, im_occ2, theta_cells, delta_ang)
% 计算霍夫频谱
HS1 = hough_spectrum(H1);
HS2 = hough_spectrum(H2);
cc_result = circular_cross_correlation(HS1, HS2);
cc_result
% 计算交叉验证后得到旋转角度，旋转后得到的图像
maxima = find_local_maxima_circular(cc_result);
maxima;
rotationEstimate = round(maxima * 360 / theta_cells);
rotationEstimate;
% 简单的卡尔曼滤波融合一下角度信息
rot_hat = kalmanFilter(delta_ang, rotationEstimate);
rot_hat
im_occ2_cc_rot = map_transform(im_occ2, rot_hat, fix([0 0]));

map2_cc_rot = get_just_occ(im_occ2_cc_rot);  
[ang2_cc_rot, R2_cc_rot, T2_cc_rot, D2_cc_rot] = align_roden(map2_cc_rot);
figure; imshow(im_occ2_cc_rot, [-1 1]);
set(gcf,'name','rot-map_cc_2','numbertitle','off');
title('aligned map_cc_2', 'FontSize',12);